#!/usr/bin/env python3

"""
ekf_mouseRobot.py - OpenCV mouse robot demo using TinyEKF

Copyright (C) 2016 Simon D. Levy

MIT License
"""
import cv2
import math
import time  # 引入time模块
import numpy as np
import random
from sys import exit

from AltitudeDataFusion import AltitudeDataFusion

LINE_AA = cv2.LINE_AA  # if cv2.__version__[0] == '3' else cv2.CV_AA

measurement_gps = [0, 104, 210, 290, 398, 499, 613, 694, 812, 909, 993, 1091, 1208, 1301, 1395, 1509, 1605, 1691, 1806, 1906, 1996, 2089, 2206, 2309, 2391, 2502, 2607, 2690, 2789, 2907]
measurement_barometers_H = [0, 75, 155, 332, 419, 509, 567, 721, 834, 938, 993, 1101, 1204, 1274, 1438, 1504, 1564, 1742, 1811, 1934, 1961, 2117, 2192, 2259, 2403, 2464, 2556, 2708, 2782, 2920]
measurement_barometers = [101.325, 100.4266099631849, 99.47544773789187, 97.39691042499965, 96.38822795303125, 95.3536816370964,
                          94.69175182921913, 92.95225021091464, 91.69239628071, 90.5451380048492, 89.94313080200484, 88.77043762058652,
                          87.66360020799254, 86.91777911154804, 85.19053188412288, 84.5033128758858, 83.88247028293578, 82.06233465092055,
                          81.36543736747255, 80.13503058455179, 79.8669686844874, 78.33234584516421, 77.6030945763177, 76.95628843940422,
                          75.58090547584266, 75.00431317586846, 74.14143986112667, 72.73344519196905, 72.05585485049728, 70.80589611085179]
measurement_imu = [5, 109, 191, 276, 428, 490, 594, 678, 774, 925, 1000, 1086, 1173, 1321, 1386, 1488, 1613, 1702, 1806, 1878, 1977, 2094, 2181, 2277, 2375, 2505, 2611, 2697, 2790, 2872]
stateXArray = []

"""
Note:  此案例仅仅依赖输入的模拟数据,产生模拟结果,无图形化界面显示.
需要查看图形曲线,请到目录下ekf_AltitudeDataFusion.xlsx查看

# 下面是某次运行本程序的输出结果
R = [[1.e-06 0.e+00 0.e+00]
    [0.e+00 1.e-02 0.e+00]
    [0.e+00 0.e+00 1.e-03]]
"""

"""
#基于 measurement_barometers:
stateXArray = [0.14953209537134615, 95.46584023034309, 200.29502957437685, 282.40686231986575, 388.65192735778544, 489.7344090308052, 
               602.568686577119, 686.2450890345611, 801.189098995206, 900.1725172278325, 985.3377062894409, 1082.1607189648387, 
               1197.2093484085533, 1292.5434152487187, 1386.3912160828295, 1498.6052924229382, 1596.2178206182107, 1683.2114311052546, 
               1795.7809829820792, 1896.5728378370507, 1987.5526023065893, 2080.6023498384834, 2195.336977486254, 2299.249965958058, 
               2383.218747691464, 2492.1417623942743, 2597.477309953391, 2682.3632538785755, 2780.1342709938444, 2896.1241138077853] """

if __name__ == '__main__':
    """
    # 测试数据 生成大气压和海拔高度对照表,进一步把数据放到excel表格生成,大气压-海拔高度关系曲线图
    values_barometers = []
    for height in range(0, 12100, 100):
        valueBarometers = 101.325 * math.pow((1 - height / 44300), 5.256)
        values_barometers.append(valueBarometers)

    print(f"height_values={list(range(0, 12100, 100))}")
    print(f"values_barometers={values_barometers}")
    """

    """
    # 测试数据,生成三个传感器的模拟数据
    altitude_gps = []
    altitude_imu = []
    altitude_bp = []
    for height in range(0, 3000, 100):
        diffHeight_gps = random.randrange(30)  # 生成模拟数据时,gps测量误差30m
        diffHeight_imu = random.randrange(60)  # 生成模拟数据时,imu测量误差60m
        diffHeight_bp = random.randrange(90)  # 生成模拟数据时,气压计测量误差90m
        altitude_gps.append(height + (diffHeight_gps-15 if (diffHeight_gps > 15) else -diffHeight_gps))
        altitude_imu.append(height + (diffHeight_imu-30 if (diffHeight_imu > 30) else -diffHeight_imu))
        altitude_bp.append(height + (diffHeight_bp-45 if (diffHeight_bp > 45) else -diffHeight_bp))

    print(f"altitude_gps={altitude_gps}")
    print(f"altitude_imu={altitude_imu}")
    print(f"altitude_bp={altitude_bp}")
    """

    """
    # 测试数据中 大气压代表高度转为大气压值,  只是为了模拟测试
    values_barometers = []
    for index in range(len(measurement_barometers_H)):
        valueBarometers = 101.325 * math.pow((1 - measurement_barometers_H[index] / 44300), 5.256)
        values_barometers.append(valueBarometers)
    print(f"values_barometers={values_barometers}")
    """

    """
    # 某次测试的一组值
    values_barometers = [101.325, 100.4266099631849, 99.47544773789187, 97.39691042499965, 96.38822795303125, 
                         95.3536816370964, 94.69175182921913, 92.95225021091464, 91.69239628071, 90.5451380048492, 
                         89.94313080200484, 88.77043762058652, 87.66360020799254, 86.91777911154804, 85.19053188412288, 
                         84.5033128758858, 83.88247028293578, 82.06233465092055, 81.36543736747255, 80.13503058455179, 
                         79.8669686844874, 78.33234584516421, 77.6030945763177, 76.95628843940422, 75.58090547584266, 
                         75.00431317586846, 74.14143986112667, 72.73344519196905, 72.05585485049728, 70.80589611085179]
    """

    # Create a new Kalman filter for mouse tracking
    kalfilt = AltitudeDataFusion(1, 3, pval=0.01, qval=1e-4, rval=0.0001)

    # 更新传感器噪声矩阵,三个传感器具有不同的噪声(测量误差).  噪音越小,最优估计值越会更接近某个传感器测量值
    R = np.eye(3)
    R[0, 0] = 0.00001  # GPS
    R[1, 1] = 0.01       # 大气压测高
    R[2, 2] = 0.001   # IMU 惯性传感器
    kalfilt.updateR(R)  # 更新传感器噪声矩阵,三个传感器具有不同的噪声(测量误差)

    print(f"R={R}")

    for index in range(len(measurement_barometers)):
        # 输入当前鼠标位置测量值,  返回新的鼠标位置最优评估值
        estimate = kalfilt.step((measurement_gps[index], measurement_barometers[index], measurement_imu[index]))  # 基于大气压
        # estimate = kalfilt.step((measurement_gps[index], measurement_barometers_H[index], measurement_imu[index]))  # 基于大气压转换前海拔高度
        stateXArray.append(estimate[0])

    print(f"stateXArray={stateXArray}")

